#include "hardware.h"
#include "keycodes.h"
#include "flags.h"
#include "var.h"

#include <util/atomic.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>

volatile WORD Flags;         // Global program state flags
volatile WORD PauseCnt;      // System pause counter x 1.25 ms
volatile WORD InpDelCnt;     // Input debounce pause counter
volatile WORD KbdDelCnt;     // Keyboard debounce pause counter
volatile DWORD StepCounter;  // Current motor step counter
volatile WORD InpPattrn;     // Current input pattern
volatile WORD InpChange;     // Last changed inputs
static volatile WORD FeedPeriod[16] = {
  2000,2000,2000,2000,2000,2000,2000,2000,
  2000,2000,2000,2000,2000,2000,2000,2000};
static volatile WORD TimeCnt = 0;     // System time counter (Feed speed)
static volatile WORD Time1S = 0;      // System time counter (1 s interval)
static volatile WORD RotorCount = 0;  // Rotor chopper count per 1 s
DWORD Counter;                 // Total wafer counter 
WORD OutMsk;                   // Output mask 1 - On, 0 - Off
BYTE WCounter;                 // Wafer counter
WORD Options;                  // Options flags;


enum OpMode Mode = _Checkout_;

static volatile char KeyCode;

WORD  e_Input     EEMEM = 0;
DWORD e_Counter   EEMEM = 0; 
DWORD e_TotalCnt  EEMEM = 0;
BYTE  e_Flags     EEMEM = 0;
BYTE  e_Mode      EEMEM = 0;
WORD  e_Output    EEMEM = 0;
WORD  e_Options   EEMEM = 0;

void Output(WORD D)
{
PORTA = SELBYTE(D,0);
PORTC = SELBYTE(D,1);
}

void SetPause(WORD T)
{
  ATOMIC_BLOCK (ATOMIC_RESTORESTATE) {
    PauseCnt = T;  
  };
}

void Pause(WORD T)
{
  SetPause(T);
  do
    {
    cli ();
    if(PauseCnt==0) break;
    sei ();
    asm ("nop");
    }
  while(1);
  sei ();
}

BYTE KeyPressed(void)
{
  BYTE result = 0;
  ATOMIC_BLOCK (ATOMIC_RESTORESTATE) {
    result = (KeyCode != 0);
  };
  return result;
}

char GetChar(void)
{
  char T;
  ATOMIC_BLOCK (ATOMIC_RESTORESTATE) {
    T = KeyCode;
    KeyCode = 0;
  };
    return T;
}

BYTE KeyDn(void)
{
KBD_ON;
PORTF = 0x0F;
asm ("nop");
asm ("nop");
BYTE D = PINF & 0xF0;
KBD_OFF;
return D;
}

 char ScanTbl[32] = {
  K_Shift,'5','A','1','9','6','B','2','0','7','C','3',K_Enter,'8',K_Esc,'4',
  K_Shift,'+','D','*','-','l','E','/','d','.','F','u',K_Enter,'r',K_Esc,K_Del};

#define KF_Shift 0x01

static BYTE KeyFlags = 0;

char ScanKbd(void)
{
BYTE J = 0, K = 0;
char D;
KBD_ON;
do{
  PORTF = 1 << J; //4321 (J=3..0)
  asm("nop");
  asm("nop");
  D = PINF & 0xF0;
  if(D == 0x10) K = 0x80 | J; else //A
  if(D == 0x20) K = 0x84 | J; else //B
  if(D == 0x40) K = 0x88 | J; else //C
  if(D == 0x80) K = 0x8C | J;      //D
  }
while(++J < 4);
KBD_OFF;
D = ScanTbl[(K & 0x0F)+(KeyFlags & KF_Shift ? 16 : 0)];
KeyFlags &= ~KF_Shift;
if(K == 0x80) KeyFlags |= KF_Shift;
return K ? D : 0;
}

/* Input */

static volatile BYTE SEL;    // selection input matrix column
static volatile WORD PIP;    // previous input pattern
static volatile WORD OIP;    // old input pattern
static volatile BYTE InpCnt; // input debounce counter

void InputInit(void) {
PORTD = ~0x10;
InpPattrn = 0;
OIP = 0;
PIP = 0xFFFF;
InpChange = 0;
SEL = 0;
InpCnt = INPUTDELAY;
}

/* System timer */
#pragma vector=TIMER0_COMP_vect
ISR(TIMER0_COMP_vect)
{
  // Inputs
  if(InpCnt-- == 0)
  {
  
  BYTE pd = PIND;
  if (pd & _BV(di_INT2))
    Flags |= F_Table;
  else
    Flags &= ~F_Table;
  
  
  InpCnt = INPUTDELAY;
  BYTE B = ~(PINE | 0x0F);
  WORD D = 0x000F;
  InpPattrn &= ~(D << (SEL*4));
  D = B >> 4;
  InpPattrn |= (D << (SEL*4));
  SEL++;  SEL &= 0x03;
  PORTD = ~(0x10 << SEL);
  if(SEL==0)
    {
    if(PIP == InpPattrn)
      {
      InpChange = OIP ^ InpPattrn;
      if(InpChange)
        Flags |= F_Input;
      OIP = InpPattrn;
      }
    PIP = InpPattrn;
    }
  }
  // Keyboard
  if(!(Flags & F_BBusy))
    {
    if(Flags & F_Keyboard)
      {
      if(KbdDelCnt)
        {
        KbdDelCnt--;
        if(!KbdDelCnt){
          Flags &= ~F_Keyboard;
          }
        }
      else
        if(!KeyDn())
          KbdDelCnt = KBDELAY;
      }
    else
      if(KbdDelCnt)
        {
        KbdDelCnt--;
        if(!KbdDelCnt){
          Flags |= F_Keyboard;
          KeyCode = ScanKbd();
          }
        }
      else
        if(KeyDn())
          KbdDelCnt = KBDELAY;
  }
  
  // Pause
  if(PauseCnt) PauseCnt--;
  // Time Counter
  if(TimeCnt < 2000) TimeCnt++;
  //
  if(++Time1S == 800)
    {
    Time1S = 0;
    RotorCount = TCNT1;
    TCNT1 = 0;
    Flags |= F_1S;
    }
}

#define MPH 3
 BYTE MP[MPH] = {0x01, 0x02, 0x04};

static signed char MPI = 0;

const char MAX_PER = 250;
const char MIN_PER = 50;

#pragma vector=TIMER2_COMP_vect
ISR(TIMER2_COMP_vect)
{

static char curr_lat = MAX_PER;

if(Flags & F_Off) {
   PORTG |= 0x03;
   curr_lat = MAX_PER;
} else
if(Flags & F_Run && (StepCounter || Flags & F_NoCnt)){
  if(Flags & F_Back){
    //if(--MPI < 0) MPI = MPH-1;
    if (Flags & F_StepperPeak) {
        PORTG &= 0xFC;
        PORTG |= 0x01;
        }
    else
        PORTG |= 0x03;
    }
  else{
    //if(++MPI > MPH-1) MPI = 0;
    if (Flags & F_StepperPeak) {
        PORTG &= 0xFC;
        PORTG |= 0x02;
        }
    else
        PORTG |= 0x03;
    }
  //PORTG = MP[MPI];
  if(!(Flags & F_NoCnt))
    if(!(--StepCounter)) {
      Flags |= F_Done;
      Flags &= ~(F_Run | F_Back);
      }
  } else { 
    PORTG |= 0x03;
    curr_lat = MAX_PER;
  };
    
  if (Flags & F_StepperPeak) {
  
    Flags &= ~F_StepperPeak;
    OCR2 = 20;
  
  } else {
  
    Flags |= F_StepperPeak;
    OCR2 = curr_lat;
    if (curr_lat > MIN_PER)
        --curr_lat;
  
  };
}

/* Power fail routine */
#pragma vector=INT0_vect
ISR(INT0_vect)
{
  Flags |= F_PowerFail;
  eeprom_write_dword (&e_Counter, Counter);
  eeprom_write_byte (&e_Flags, SELBYTE(Flags,1));
  eeprom_write_byte (&e_Mode, Mode);
  eeprom_write_word (&e_Output, OutMsk);
  eeprom_write_word (&e_Input, InpPattrn);
  PauseCnt = 400; // 500 ms pause
}

void Switch(WORD M, enum BITSTATE S)
{
  if(S==ON)
    OutMsk |= M;
  else if (S==OFF)
    OutMsk &= ~M;
  else
    OutMsk ^= M;
  Output(OutMsk);
}

#pragma vector = INT1_vect
ISR(INT1_vect)
{
  static BYTE i=0;
  if(TimeCnt >= 6) FeedPeriod[i++] = TimeCnt;
  i &= 0x0F;
  TimeCnt = 0;
}
static volatile char feedspeedflag=1;
static volatile WORD fs1;
static volatile WORD fs2;
static volatile WORD fs3;

DWORD AvarageFeedSpeed(void)
{
    DWORD q;
    q=fs1+fs2+fs3;
//if (fs1 && fs2 && fs3){
    return q/3;
//}else return 0;    
    
}

WORD FeedSpeed(void)
{
BYTE i;
WORD S;
WORD R;
if(TimeCnt < 2000)
  {
    cli();
  for(i=0, S=0; i<16; i++)
    {
    //cli ();
    S += FeedPeriod[i];
    //sei ();
    }
    sei();
  R=4 * 3650/S;
  return R;//(16*60.0*800.0/33.75)/S;
  }
else
  return 0;
}

// 90 holes 1:7.5 gear 20 mm/rev -> 1mm <=> 33.75 holes
// FS[mm/min] = (800*60/33.75)/FeedPeriod
